#include "actuator/actuator.h"

using namespace actuator;

std::shared_ptr<cls_lib> ActuatorMain::ptr_vcs_ = nullptr;
bool ActuatorMain::running = true;

ActuatorMain::ActuatorMain(ros::NodeHandle &nh, ros::NodeHandle &private_nh, std::string file, std::string log_path)
: nh_(nh),
  private_nh_(private_nh),
  log_path_(log_path)
{
    InitLog();
    LINFO("======================================actuator start======================================");
    signal(SIGINT,  ActuatorMain::SigHandler);
    signal(SIGTSTP, ActuatorMain::SigHandler);

    read_buffer_  = std::make_shared<boost::circular_buffer<st_par>>(10);
    write_buffer_ = std::make_shared<boost::circular_buffer<st_par>>(10);
    
    ptr_kinematic_ = std::make_shared<AMRKinematicModeNormal>();
    ptr_params_ = std::make_shared<GetParamClient>();
    
    ptr_params_->Init(file, read_buffer_);
    std::string libpath;
    if (ptr_params_->GetParam("libpath", libpath))
    {
        libpath = libpath + "/libvcs.so";
        ptr_vcs_.reset();
        ptr_vcs_ = std::make_shared<cls_lib>(libpath);
        ptr_vcs_->Init(read_buffer_, write_buffer_);
        v_futures.emplace_back(std::async(std::launch::async | std::launch::deferred, &cls_lib::RunTick, ptr_vcs_));
    }
    else
    {
        LWARNING("Could Not Find libvcs.so");
    }

    sub_control_ = nh_.subscribe("/control", 10, &ActuatorMain::ControlCallback, this);
    pub_actuator_ = nh_.advertise<actuator_msgs::actuator_msg>("/actuator", 10);
    // common_lib::LibTime::sleep(1);
    printf("actuator Init\n");
    Init(file);
    ROS_INFO("read cnt: %ld, wirte: %ld", read_buffer_.use_count(), write_buffer_.use_count());
}

ActuatorMain::~ActuatorMain()
{
    for (auto&& f : v_futures)
    {
        f.get();
    }
}

void ActuatorMain::Init(std::string file)
{
    ptr_params_->LoadAMRParams("servo", amr_state.steer_param);
    SteerState tmp;
    tmp.steer_speed = 0.0f;
    tmp.steer_angle = 0.0f;
    amr_state.steer_state.push_back(tmp);
    amr_state.steer_state.push_back(tmp);

    ptr_params_->GetParam("loop", loop_);
}

void ActuatorMain::InitLog()
{
    common_lib::SpdLog::Instance()->InitSpdLog(log_path_ + "/log", "actuator"); 
    // TsdpClientLog::GetInstance().Init(); // TODO CSVlogForLater
}

void ActuatorMain::Start(void)
{
    ros::Rate loop(loop_);
    printf("Start() loop: %d\n", loop_);
    while (running)
    {
        // ROS_INFO("Start, read size: %ld, write: %ld", read_buffer_->size(), write_buffer_->size());

        GetState();
        CalcSpeed();
        loop.sleep();
        ros::spinOnce();
    }
}

void ActuatorMain::ControlCallback(const control_msgs::control_msgConstPtr &msg)
{
    msg_control_ = *msg;
}

void ActuatorMain::GetState()
{
    if (read_buffer_->empty())
        return;
    
    while (!read_buffer_->empty())
    {
        struct st_par par = read_buffer_->front();
        read_buffer_->pop_front();
        amr_state.steer_state.clear();
#if 0
        for (int i=0;i<amr_state.steer_param.size();i++)
        {
            SteerState state;
            state.steer_angle = 0.0f; // read_buffer_->front().drv_servo;
            state.steer_speed = 0.0f;

            amr_state.steer_state.emplace_back(state);
        }    
#else
        amr_state.steer_state.resize(2);
        amr_state.steer_state[0].steer_speed = par.drv_pv_2 * 0.001f;
        amr_state.steer_state[0].steer_angle = com_.Degree2Radian(0.0f);

        amr_state.steer_state[1].steer_speed = par.drv_pv_1 * 0.001f;
        amr_state.steer_state[1].steer_angle = com_.Degree2Radian(0.0f);

        ptr_kinematic_->VehicleInverseMode(amr_state, msg_actuator_.walking_speed, msg_actuator_.angle_speed, msg_actuator_.speed_toward);

        printf("AMR velocity: %f, w: %f, toward: %f, steer speed: %f %f\n", 
                msg_actuator_.walking_speed, msg_actuator_.angle_speed, msg_actuator_.speed_toward, 
                amr_state.steer_state[0].steer_speed, amr_state.steer_state[1].steer_speed);
        msg_actuator_.header.stamp = ros::Time::now();
        msg_actuator_.is_auto_mode = true;
        pub_actuator_.publish(msg_actuator_);
#endif
    }
}

void ActuatorMain::CalcSpeed()
{
#if 0
    eMoveMode mode = eMoveMode::STILL;
    if (msg_control_.running_type == eRunningType::HEADFOLLOWING)
    {
        mode = eMoveMode::HEADFOLLOW;
    }
    else if (msg_control_.running_type == eRunningType::ROTATING)
    {
        mode = eMoveMode::ROTATE;
    }
    else if (msg_control_.running_type == eRunningType::STEERTURNING)
    {
        mode = eMoveMode::TOWARD;
    }
    amr_state.steer_cmd.clear();
    ptr_kinematic_->KinematicMode(msg_control_.speed, msg_control_.angle_speed, msg_control_.speed_toward, mode, amr_state);
#else
    // msg_control_.speed = 0.0f;
    // msg_control_.angle_speed = com_.Degree2Radian(5.0f);
    struct st_par wpar;
    wpar.rc_sv_x = (int)(msg_control_.speed * 1000.0f);
    wpar.rc_sv_y = 0.0f;
    wpar.rc_sv_z = com_.Radian2Degree(msg_control_.angle_speed);
    wpar.b_auto_mode = 0;
    write_buffer_->push_back(wpar);
#endif
}

void ActuatorMain::SigHandler(int sig)
{
    ptr_vcs_.reset();
    running = false;
    printf("SigHandler: %d\n", sig);
}

